//
// Created by f on 2020/4/8.
//
//读取一张图片后，if(is_initialized = false || bad_fit_count > 10

#include "lane_class.h"

lane_class::lane_class() {
    is_initialized = false;
}

void lane_class::init(const cv::Mat &img) {
    bad_fit_count = 0;
    cur_left_fit.clear();
    cur_right_fit.clear();

    cv::Mat undt;
    undist(img, undt);

    cv::Mat white;
    cv::Mat yellow;
    whiteThresh(undt, white);
    yellowThresh(undt, yellow);
    cv::Mat color_selected;
    cv::bitwise_or(white,yellow,color_selected);

    vector<cv::Mat> img_binary(color_selected.channels());
    cv::split(color_selected, img_binary);

    cv::Mat warped;
    perspectiveTransform(img_binary[0],warped,false);

    cv::Mat cannyed;
    canny(warped, cannyed);

    //cv::Mat houghed;
    //hough(cannyed, houghed);

    vector<cv::Point2i> temp_left_pts;
    vector<cv::Point2i> temp_right_pts;
    findPixels0(cannyed, temp_left_pts, temp_right_pts);
    if(temp_left_pts.size() > 10 && temp_right_pts.size()>10){
        left_pts = temp_left_pts;
        right_pts = temp_right_pts;
        left_fit = fitLane(left_pts,2);
        right_fit = fitLane(right_pts,2);
        is_initialized = true;
        bad_fit_count = 0;
    }
    else{
        is_initialized = false;
        left_fit = {W+IMG_COLS/3, 0, 0};
        right_fit = {W+IMG_COLS/3*2, 0, 0};
    }
}

void lane_class::update(const cv::Mat &new_img) {
    cv::Mat new_undt;
    undist(new_img, new_undt);

    cv::Mat new_white;
    cv::Mat new_yellow;
    whiteThresh(new_undt, new_white);
    yellowThresh(new_undt, new_yellow);
    cv::Mat new_color_selected;
    cv::bitwise_or(new_white,new_yellow,new_color_selected);

    vector<cv::Mat> new_binary(new_color_selected.channels());
    cv::split(new_color_selected, new_binary);

    cv::Mat new_warped;
    perspectiveTransform(new_binary[0],new_warped,false);

    cv::Mat new_cannyed;
    canny(new_warped, new_cannyed);

    //cv::Mat new_houghed;
    //hough(new_cannyed, new_houghed);

    vector<cv::Point2i> new_left_pts;
    vector<cv::Point2i> new_right_pts;
    findPixels1(new_cannyed, new_left_pts, new_right_pts, left_fit, right_fit);
    bool left_bad_fit=false;
    bool right_bad_fit=false;
    if(new_left_pts.size() > 150 && new_right_pts.size() > 150){
        vector<float> new_left_fit = fitLane(new_left_pts,2);
        float left_diff = abs(left_fit[0]+left_fit[1]*720+left_fit[2]*720*720 - new_left_fit[0]+new_left_fit[1]*720+new_left_fit[2]*720*720);
        if(left_diff<50 && abs(left_fit[0]-new_left_fit[0])<300){
            cur_left_fit.push_back(new_left_fit);
            if(cur_left_fit.size()>max_saved_fit) cur_left_fit.pop_front();
            vector<float> sum_left_fit(3);
            for(auto& lf:cur_left_fit){
                sum_left_fit[0] += lf[0];
                sum_left_fit[1] += lf[1];
                sum_left_fit[2] += lf[2];
            }
            left_fit = {sum_left_fit[0]/cur_left_fit.size(), sum_left_fit[1]/cur_left_fit.size(), sum_left_fit[2]/cur_left_fit.size()};
        }
        else{
            left_bad_fit = true;
        }

        vector<float> new_right_fit = fitLane(new_right_pts,2);
        float right_diff = abs(right_fit[0]+right_fit[1]*720+right_fit[2]*720*720 - new_right_fit[0]+new_right_fit[1]*720+new_right_fit[2]*720*720);
        if(right_diff<50 && abs(right_fit[0]-new_right_fit[0])>300){
            cur_right_fit.push_back(new_right_fit);
            if(cur_right_fit.size()>max_saved_fit) cur_right_fit.pop_front();
            vector<float> sum_right_fit(3);
            for(auto& lf:cur_right_fit){
                sum_right_fit[0] += lf[0];
                sum_right_fit[1] += lf[1];
                sum_right_fit[2] += lf[2];
            }
            right_fit = {sum_right_fit[0]/cur_right_fit.size(), sum_right_fit[1]/cur_right_fit.size(), sum_right_fit[2]/cur_right_fit.size()};
        }
        else{
            right_bad_fit = true;
        }
    }
    else{
        left_bad_fit = true;
        right_bad_fit = true;
    }
    left_bad_fit && right_bad_fit ? (bad_fit_count += 1) : (bad_fit_count = 0);
}